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ABSTRACT 


Cooperative Localization (CL) is a process by which autonomous vehicles operating as a 
team estimate the position of one another to compensate for errors in the positioning sen¬ 
sors used by a single agent. By combining independent measurements originating from 
members of the team, a single estimate of increased accuracy will result. This approach 
has the potential to enhance the positional accuracy of an agent over use of a standard GPS, 
which would be essential for behaviors within a swarm requiring precision movements such 
as maintaining close formation. CL can also provide accurate positional information to the 
entire group when operating in an intermittent or denied GPS environment. In this the¬ 
sis, a distributed CL algorithm is implemented on a swarm of Unmanned Aerial Vehicles 
(UAVs) using an Extended Kalman Filter. Using a technique created for ground robots, the 
equations are modified to adapt the algorithm to aerial vehicles, and then operation of the 
algorithm is demonstrated in a centralized system using AR Drones and the Robot Operat¬ 
ing System. During tests, the positional accuracy of the UAV using CL improved over use 
of dead reckoning. However, the performance is not as expected based on the results noted 
from the referenced two-dimensional application of the algorithm. It is presumed that the 
sensors on-board the AR Drone are responsible. Since the platform is simply a low-cost 
solution to show proof-of-concept, it is concluded that the implementation of CL presented 
in this thesis is a suitable approach for enhancing positional accuracy of UAVs within a 
swarm. 
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CHAPTER 1: 
Introduction 


1.1 Motivation 

1.1.1 The Expansion of Unmanned Aerial Vehicle Mission Sets 

Unmanned Aerial Vehicles (UAVs) have become an essential tool for the execution of mil¬ 
itary plans and securing our national defense. This fact is evident by the 40 fold increase 
in the Department of Defense (DOD) unmanned aircraft inventory from 2002 to 2010 [1]. 
Similar to the expansion of military manned aircraft mission types from World War I to 
World War II, UAV mission types are also expanding. With thousands of successfully 
completed combat- and noncombat-related missions supporting intelligence, command and 
control, targeting, and limited weapons delivery, these systems have gained the confidence 
of military and government leaders. Development of UAVs has been accelerated by new 
technologies in miniaturization, the relatively low cost of manufacturing, and the devel¬ 
opment and distribution of open-source control software. Advances in navigation and 
communications technologies have also played a role in allowing operations of UAVs to 
be more practical by significantly increasing their operational range [1]. This increased 
confidence in UAVs along with recent matured technologies has fostered efforts aimed at 
increasing their capabilities and expanding their current mission set. 

Unmanned systems are not limited to the United States. Other international governments 
are also actively developing and deploying these systems, including potential adversaries 
such as Iran and China. Doctrinal discussions in light of rapid innovation and development 
have focused on the feasibility and utility of “swarm tactics,” where small distributed units 
converge rapidly on particular targets [2]. Specifically, research has been conducted to 
combat Harpy UAV swarm attacks, where an overwhelming number of UAVs attack a high 
value target by saturating air defense systems [3]. One way to counter such swarm attacks 
is to intercept the enemy swarm with our own swarm of UAVs. 
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1.1.2 Swarming 

Swarming is a collective behavior or motion exhibited by a large number of self-propelled 
entities [4]. For example, an ant colony as a whole appears to be highly organized, yet 
each ant seems to have its own agenda [5]. Benefits of this collective behavior include 
completion of complex tasks, redundancy against the failure of a single entity, a decrease 
in time to complete certain tasks, and an increase in efficiency through cooperation. Scien¬ 
tists have found that coordination through social species arises through interactions among 
individuals and not from a hierarchical level. These simple interactions together can solve 
difficult problems such as finding the shortest route to a food source. This type of behavior 
is often a basic principle behind self-organizing systems and emergence where “organiza¬ 
tion” is related to an increase in the structure or order of behavior, and emergence is where 
properties, behavior, or patterns with the system as a whole dynamically arise from the 
novel interactions between individual parts of the system [6]. Over the years, there has 
been significant interest in this natural behavior, which has been dubbed “swarm intelli¬ 
gence” [5]. The works of [7], [8] and [9], just to name a few, have dedicated their efforts to 
understanding and modeling this behavior so that it can be applied to many other diverse 
tasks and complex problems. For example, the foraging of ants has led to a novel method 
for rerouting network traffic in busy telecommunications systems, and the division of labor 
among honeybees could help streamline assembly lines in factories [5]. 

The term “swarming” is also used by military historians to describe a battlefield tactic that 
involves decentralized, pulsed attacks. As stated by Parunak, the link between the two uses 
of the word is not coincidental. “Insect self-organization is robust, adaptive, and persistent, 
as anyone can attest who has tried to keep ants out of the kitchen or defeat a termite infes¬ 
tation, and military commanders would love to be able to inflict the frustration, discomfort, 
and demoralization that a swarm of bees can visit on their victims.” [10]. As discussed 
in [11], the swarming tactic is a structured and coordinated way to attack an enemy from all 
directions by a sustainable pulse of force from stand-off and close-in positions. Swarming 
depends on a centralized strategy that is designed around a number of small, dispersed, and 
networked maneuver units that are allowed to operate in a decentralized manner. Swarm¬ 
ing depends heavily on the operation of integrated information systems that can distribute 
specific targeting information as well as other information to update battlespace awareness. 
Examples of swarming are found throughout history, but it is now able to emerge as a doc- 
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trine for UAVs because advances in information technology have given us the ability to 
provide the required interconnection between the units [11]. 

For actions against the enemy in an objective area, the swarming tactic can be conceptually 
broken into four stages: locate the target, converge on it, attack, and disperse. Agents within 
the swarm must be capable of sustaining multiple waves of attack, quickly merging with 
other agents on a target, then redispersing and recombining for a new wave [2]. During the 
converging stage, agents within a swarm must come together to form a whole. However, 
some form of control is necessary to ensure agents do not collide while continuing to steer 
toward the target. The most basic form of control has been modeled and is based on the 
behaviors of flocking birds. First simulated by Craig Reynolds, the Boids model contains 
the following rules for each agent within the flock: avoid crowding neighbors (separation), 
steer toward the average position of neighbors (long-range cohesion), and steer toward 
the average heading of neighbors (alignment goal vector) [7]. Although effective in the 
simplest of terms regarding the convergence of agents within a swarm, more structure may 
be required in a formation control scheme to be effective in the interception of another 
swarm. 


1.2 Problem Statement 

A Congressional Budget Office study in 2011 compared the costs of DOD plans and the 
capabilities those plans might provide for reconnaissance and light attack missions with 
the costs and capabilities of some alternate options [12]. Not surprisingly, services within 
the DOD showed that systems with higher capability come at an increased cost. Although 
UAVs are usually less expensive than manned aircraft, many of the advanced systems and 
sensors carried by some UAVs to perform with increased capabilities are very expensive 
and cannot be viewed as expendable. Also, high levels of reliability to ensure safety of 
people on the ground and in other aircraft are required. This means additional and often 
expensive systems such as sense-and-avoid cameras, transponders, and other improvements 
in airworthiness and reliability are added to UAVs. Finally, support systems, such as ground 
stations, add costs that are not associated with manned aircraft [12, p. 31]. Therefore, 
successfully applying swarm tactics to UAVs involves development of as many expendable 
systems as possible to be cost effective. In the design and development of such systems, the 
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requirement to “do more with little” by creating small, simple, lightweight, and low-cost 
UAVs is a primary concern and major focus of effort. 

1.2.1 Localization in Swarm UAVs 

The problem of localization, also referred to as position estimation, is central in mobile 
robotics [13], and the need for accurate positional information is crucial to not only reduce 
the probability of collision between agents in a swarm but also to conduct its mission. No 
matter how many agents make up the swarm, the primary factor that determines the un¬ 
certainty of the position estimates of an agent is the accuracy of its proprioceptive sensors 
and orientation estimates [14]. To increase the positional accuracy of a UAV, incorporating 
more precise sensors such as a Differential Global Positioning System (DGPS) or an Em¬ 
bedded Global Positioning System/Inertial Navigation System (EGI) can be used. These 
systems as well as other techniques to improve single UAV navigation and reduce their 
uncertainty are further discussed in Sections 2.1 and 2.2. Because DGPS and EGI sensors 
are typically costly and heavy, another solution may be more applicable to swarm UAVs. 
Mahboubi et al. demonstrate the use of a camera-based system for measuring the rela¬ 
tive position of one aircraft to another in [15]. Because high quality camera systems have 
become lighter and less expensive, they are ideal for UAVs over such systems as DGPS, 
which require both receivers to be moving over the ground, are very heavy, and typically 
cost much more than other systems [15]. Using a camera-based sensor to determine the 
relative position of another aircraft in the swarm as well as applying some form of cooper¬ 
ative localization may attain the same amount of positional precision as a DGPS or EGI at 
a more reasonable cost. 

1.2.2 Cooperative Localization 

Cooperative Eocalization (CE) is a term used to describe the technique where agents within 
a team estimate the position of one another. The relative position estimates acquired by 
an agent can be used to compensate for errors in odometry or other pose and positioning 
sensors by combining measurements [16]. By properly combining a number of independent 
measurements originating from the different members of the team, a single estimate of 
increased accuracy and reduced uncertainty results. The better estimate of the position and 
orientation of a landmark can significantly improve the outcome of the localization process, 
and thus, the entire group of robots can benefit from the collaboration [17]. There are 
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numerous algorithms and techniques that have been developed to conduct multi-agent CL 
to include use of probabilistic and statistical estimation. These approaches are discussed 
further in Section 2.3.1. 

Applying CL to swarm UAVs would allow an agent with no or degraded Global Positioning 
System (GPS) solution to determine positional information necessary to carry out missions 
within the objective area as well as improve the location estimates of all agents. Based 
on preliminary research in this field, there is no evidence of CL being implemented in 
swarm UAVs to improve positional accuracy and no implementation or operationally real¬ 
istic demonstration in aerial domains. The demonstration of CL by [13] and [18] in ground 
robots provides a firm base from which to move forward; however, other approaches may 
be more applicable for implementation in swarm UAVs. Since the goal of swarm aircraft 
is to be smaller, less expensive, and easier to procure than their larger counterparts, agents 
will nominally have limited computational power, communications bandwidth, and payload 
space. Therefore, techniques used to improve positional accuracy must account for these 
limitations. Use of Kalman filters is common in air vehicle applications; however, direct 
implementation of the Kalman filtering algorithm is not efficient [19]. Many matrix multi¬ 
plications and inversions involved in the algorithm make it computationally complex [19, p. 
61]. It is important to note that computational resources are continuing to get smaller and 
more powerful, allowing realization of more sophisticated approaches for conducting CL. 

1.2.3 Research Questions 

Research questions explored in this thesis include: 

1. Can a CL scheme be implemented on a swarm of UAVs to improve not only the 
positional accuracy of a single agent but also the localization of the entire swarm? 

2. Specifically, can modifications to the approach demonstrated in [20], which uses an 
Extended Kalman Filter (EKE), be made to conduct CE on a swarm of UAVs? 

3. What are the challenges and considerations in successfully implementing CE in the 
domain of UAVs? 
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1.3 Benefits of the Study 

This thesis demonstrates a means to improve overall loealization of any land, sea, or 
undersea-based robots working on higher-level tasks that involve team eoordination with 
one another. This improvement ean be made with robots using noisy or degraded propri- 
oeeptive and exteroeeptive sensors. This thesis also serves as another step in the overall 
assessment of eore swarm eapabilities and limitations as they evolve and beeome oper¬ 
ationally relevant to DOD missions. Extensions of this work inelude the exploration of 
eapabilities of distributed systems and advanees in on-board autonomous eapabilities for 
distributed agents. 

1.4 Thesis Organization 

The next ehapter reviews existing single UAV navigation teehniques while also highlight¬ 
ing some other loealization teehniques eommon in the field of roboties. Chapter 2 also 
introduees the eoneept of CL to inelude eentralized and distributed approaehes while also 
highlighting some of the previous work done in CL. In Chapter 3, an implementation of 
CL using an EKL in a two-dimensional (2-D) spaee of motion is presented, and further 
modified to allow for CL in UAVs by adding eonsiderations for altitude (3-D). Chapter 4 
outlines the method for demonstrating the modified CL algorithm by utilizing the Robot 
Operating System (ROS) and the Parrot AR Drone aerial robot. Results and analysis of the 
experiment are shown and diseussed in Chapter 5. Linally, eonelusions, reeommendations, 
and future work are highlighted in Chapter 6. 
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CHAPTER 2: 

Single and Cooperative Localization 


2.1 Uncertainty in Single UAV Navigation Techniques 

There are five common sources of error that lead to uncertainty in localization. They in¬ 
clude environmental dynamics, random action effects, inaccurate models, sensor limita¬ 
tions, and approximate computation. This section is focused on highlighting these uncer¬ 
tainties and the trade-offs made among commonly used localization methods in UAVs. The 
goal is also to summarize many of the techniques available to localize a UAV while empha¬ 
sizing the advantages and disadvantages of each. While researching each of the techniques, 
it quickly becomes apparent that no single localization method is applied in real-world au¬ 
tonomous systems that require a navigational solution. Rather, many methods must be 
combined or integrated to produce a robust and more accurate measurement. The cost of 
sensors, computational complexity, amount of storage, size and weight constraints, power 
consumption, dependency on external resources, knowledge of the operating environment, 
and desired accuracy are all trade-offs when considering the right localization method for 
the type of system being considered. It is also important to note that different localization 
techniques are used based on the type of problem they are trying to solve. Tracking or 
local techniques aim at compensating odometric errors that occur during navigation. They 
require an approximate location of the robot initially, and they typically cannot recover if 
they lose track of the system’s position. There are also localization techniques that focus 
on global approaches. These methods can localize a system without prior knowledge about 
position and can recover when localization has been interrupted or positioning errors have 
been encountered. Since the focus of this thesis is on swarm UAVs, the objective is to use 
the methods that will minimize all the error from sources listed above while maximizing 
the accuracy for each agent within the swarm. 

2.1.1 GPS and DGPS 

The most common navigational aid is GPS. The geographical latitude and longitude as 
well as the elevation above sea level is calculated by determining the distances to at least 
three GPS satellites. Satellite position is known at all time by various observational and 
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orbital computational methods. When one distance is known, the user must be located on 
the surface of a sphere with the satellite at the center and with a radius equal to this dis¬ 
tance. With two distances, the location must be on a circle that represents the intersection 
between the two spheres. With three distances known, two points are possible of which 
one will be far out in space and can be eliminated. Thus, the point on the surface has 
been determined. Over the years, GPS receivers have become cheaper, smaller, and can be 
found in everything from commercial aircraft to cell phones. However, most GPS receivers 
operate using the Standard Positioning Service (SPS) where it is only possible to reach a 
15-meter accuracy in the best case [21]. Military users who have specially equipped re¬ 
ceivers and the required cryptographic equipment and keys can use the Precise Positioning 
Service (PPS), where one meter accuracy or better can be achieved [21]. GPS, however, is 
prone to errors, and at times is unreliable due to a combination of noise, bias, and blunders. 
These errors mean the predictable accuracy of SPS is 100 meters (m) horizontal and 156 m 
vertical while PPS users observe a predictable accuracy of 22 m horizontal and 27.7 m ver¬ 
tical [22]. Sources of error in GPS include: measurements in signal arrival time, ephemeris 
and clock data, numerical calculations, atmospheric effects (i.e., temperature, pressure, hu¬ 
midity, and ionization), multi-path signals (range measurement), and natural interference. 
Control segment mistakes and receiver errors from software or hardware failures also lead 
to errors. The magnitude of errors depend on the geometric Dilution of Precision (DOP). A 
high DOP results when visible satellites are close, causing a small angular separation. Low 
DOP results when visible satellites are farther away, causing a higher angular separation 
and better positional accuracy [22]. Furthermore, since GPS signals at the receivers tend to 
be rather weak, signals can easily be blocked by buildings and other high obstacles, and are 
vulnerable to jamming. If signals are not blocked, these conditions can still produce local 
errors where signal reflection from structures can produce multipath errors and interference 
can be introduced into the signal from other stronger local radio signals. 

Differential positioning aims to correct GPS bias errors at one location with measured bias 
errors at a known position. DGPS determines a localized pseudorange correction factor by 
computing the difference between positions indicated by the GPS satellites and known fixed 
positions. Positional accuracy can be further improved by utilizing the carrier phase from 
the LI and L2 channels of the satellite signal and using carrier-phase differencing [23]. This 
approach requires the remote receiver to use dual channels to compare the reference signal 
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from the base station and the GPS satellite to determine the signal phase difference between 
the two. After they are compared, a range is determined by adding the phase difference to 
the total number of waves that occur between each satellite and the receiving antenna. 
The digital correction is broadcast locally over short range ground based transmitters. The 
most common DGPS is run by the United States Coast Guard and uses longwave radio to 
transmit the correction signals near major waterways and harbors. In [23], local differential 
services were reported to provide one to three foot accuracy while dual channel receivers 
can provide accuracy between three and twelve inches. Although DGPS has the potential 
to provide the accuracies needed to sustain close formation flight for swarm UAVs, flight 
of the aircraft would be limited to areas where ground stations are located. Also, typical 
DGPS systems equipped with dual-channel receivers presently range in price from $2,500 
to $7,500 [23]. 

Real Time Kinematic (RTK) is another augmentation to GPS that uses a ground base station 
to correct for positional errors generated by the system. RTK uses measures of the phase 
in the satellite signal’s carrier wave rather than the information content of the signal and 
relies on a single reference station to provide real-time corrections. RTK systems are the 
most precise of all GPS systems because they can provide accuracy within one to four 
centimeters and can achieve complete repeatability [23]. However, similar to DGPS, RTK 
requires a fixed ground station. Also, the cost of a full RTK system to include base station, 
rover, data logger, and software is often more than $40,000 [23]. 

2.1.2 Inertial Navigation 

An Inertial Navigation System (INS) uses measurements provided by accelerometers and 
gyroscopes to track the position and orientation of an object relative to an initial starting 
point, orientation and velocity. Inertial Measurement Units (IMUs) use orthogonal rate- 
gyroscopes and accelerometers to measure angular velocity and linear acceleration in three 
axes. The resulting measurements can then be processed to track the position and orien¬ 
tation of a device using Dead Reckoning (DR). Unlike GPS, an INS is self-contained. It 
requires no external infrastructure, is available anywhere (e.g., inside buildings, close to 
large obstacles, and underwater), and is robust to jamming [24]. The gyroscopes and ac¬ 
celerometers in an INS can be extremely precise in measuring velocity and acceleration, 
and in [25] and [24], the most common gyroscope and accelerometer types are compared. 
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Even though measurements are preeise in an INS, they are still prone to errors that originate 
from white noise, temperature, calibration errors, flicker noise, and bias. Error introduced 
in the computed orientation can cause the body-frame acceleration signals obtained from 
the INS to be projected incorrectly onto the global axis. This results in accelerations of 
the device being integrated in the wrong direction. Also, acceleration due to gravity can 
no longer be correctly removed, which results in an accumulation in position error when 
the signals are subsequently integrated. Since a rotation matrix obtained from an attitude 
algorithm is used to project the acceleration signals into global coordinates, errors in the 
angular velocity signals cause drift in the calculated position. Advances in the development 
of Micro-Machined Electromechanical System (MEMS) devices have made it possible to 
manufacture an INS that is small and lightweight. MEMS inertial sensors that include 
magnetometers to reduce drift in orientation can be used to construct inertial navigation 
systems, which suffer average drifts of around 5 m after 60 seconds (sec) of stationary 
operation. In addition, it is not currently possible to construct an INS which maintains sub¬ 
meter accuracy for more than 60 seconds using MEMS devices [26]. Drift can be reduced 
by fusing other sensors such as GPS and magnetometers. Drift can also be reduced by 
exploiting constraints that are known to apply to the movement of the IMU, such as known 
points in time at which the device must have a zero velocity. In [24], the INS/GPS cost as a 
function of inertial instrument technology and performance is shown. It is no surprise that 
more precision with use of ring laser or fiber-optic gyros comes at a higher cost; however, 
these systems will never be fully free from errors caused by drift and having a continuous 
or reliable GPS signal can not be guaranteed. 

2.1.3 Terrain-Based Navigation 

Another self-contained technique for UAVs to derive positional reference information is 
through terrain-based navigation. This form of localization can be broken into four methods 
and are described in detail in this section. The first, which has been used in the navigation of 
cruise missiles, compares terrain elevation models to measured ground elevation profiles. 
Next, sequences of on-board images are matched to geo-referenced images. The third 
method locks onto objects in the center of a camera image and tracks them in subsequent 
images. Image displacement is sensed in subsequent images and UAV motion between 
images is determined to incrementally update position and velocity. The final method uses 
optical flow, which is the measurement of image velocity. The sensors used to perform 
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terrain-based navigation can vary; however, focus in this thesis is on the potential use of 
video images for the last three methods. Compared to other sensors such as laser or radar, 
cameras are typically lighter, cheaper, easier to acquire, and consume less power. Color 
images also contain a large amount of information which can be used for various purposes 
such as feature or object identification. However, it is also important to note that cameras 
are sensitive to lighting conditions, affected by glare, and because they are limited to the 
visual spectrum, cannot visually represent a point of interest if it is obstructed by another 
object (e.g., a camera cannot see a target if it is obstructed by cloud cover). 

Conceptually, the simplest of the terrain-based navigation methods is Terrain Contour Map¬ 
ping (TERCOM) which is discussed in [27]. TERCOM compares a measured terrain profile 
to terrain profiles stored in the system computer and determines the geographic location of 
the measured profile by the best match. The system uses the assumption that a single ge¬ 
ographic location on land is uniquely defined by the contours of the surrounding terrain. 
The measured terrain profile is determined by combining the altitude measurements of a 
radar and barometric altimeter along with the vertical accelerometer output of an IMU. 
The reference terrain profile consists of a digital representation of those elevation profiles 
over which the system is most likely to fly. The computer correlates the measured profile 
against each profile on the reference map. The reference point that matches best should 
be the one representing the terrain over which the missile flew. When a position error is 
noted, TERCOM sends a command to correct for drift error in the inertial system and the 
missile is given commands to correct its course. TERCOM depends on a reference map 
to be successful, which means it must know the environment for which it is operating. 
Also, TERCOM requires terrain that is sufficiently unique at each fix site and performs 
best where changes in elevation are large. Another technique in this category uses aerial 
images. In [28], a localization algorithm uses a multiple aerial image sequence to recover 
elevations from feature points in the images. These elevations are then compared with data 
from the digital elevation model (DEM) to estimate ground position. 

Matching sequences of on-board images to geo-referenced images is essentially a more 
complex form of TERCOM. Here, instead is using altitude sensors and maps represent¬ 
ing elevation, video images are being used. An image registration technique based on 
edge matching has been demonstrated in [29]. A Sobel edge detector is applied to both 
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a geo-referenced image and an image taken from the camera aboard a UAV. A match¬ 
ing algorithm then tries to find the position in the cropped reference image which gives 
the best match with the camera image. The position that results in the greatest number 
of overlapping pixels between the edges of the two images is taken as a matching result. 
The absolute position of the UAV can be calculated since the on-board image can be geo- 
referenced. Similar to TERCOM, matching images to geo-referenced images limits the 
operating environment and is bound to the stored reference data. 

The next category of terrain-based navigational methods is the measurement of image dis¬ 
placement between multiple camera images, known as visual odometry. In [29] and [30], 
algorithms are described that update UAV position by locking onto a feature or features in 
a camera image and tracking them in subsequent images. Relying on the feature’s ground 
location, the algorithms sense the feature location in the UAV frame. Along with attitude 
measurements taken from on-board angular sensors, the feature coordinates in the ground 
and UAV frames are used by the algorithms to compute the UAV’s ground frame position. 

Optical flow, as defined in [31], refers to the apparent movement of texture in a visual 
field resulting from motion. This information can be used to perceive relative depths of 
visible objects and self-motion such as speed and rotation. An example of optical flow is 
a bird flying low to the ground. When looking down, the bird would see objects moving 
faster through its visual field compared to those objects it observes while flying higher. 
In the forward direction, obstacles can be detected by detecting expansion or divergence 
of the obstacle. More rapid expansion of objects implies a closer proximity to the object. 
Barrows explains that the “focus of expansion” (FOE) from which optical flow originates 
can indicate the direction of heading. If the FOE is located inside a rapidly expanding 
object, collision with that object is imminent. However, if the FOE is located outside 
an expanding region, approach toward the object will be close but collision with the object 
would be avoided. The three stages of optical flow processing are as follows: prefiltering or 
smoothing to extract interest points and enhance the signal-to-noise ratio, the extraction of 
local correlation surfaces or space-time derivatives, and the integration of measurements to 
produce a 2-D flow field. A summary of the constraints and regularization of the estimation 
of optical flow can be found in [32]. A survey of differential, region-based matching. 
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energy-based, and phase-based techniques can be found in [33]. A more current evaluation 
and the latest benchmark for evaluating optical flow algorithms can be found in [34]. 

In [35], the feasibility of estimating the velocity of a ground robot using optical flow was 
studied. To estimate vehicle velocity, an optical flow algorithm is used to first estimate a 
flow field from an image pair. The resulting flow is then multiplied by the camera’s frame 
rate to get an image velocity. The image velocity is then converted to vehicle velocity by 
multiplying the distance between the feature point and the camera center of projection and 
dividing by the camera focal length. 


2.1.4 SLAM 

Simultaneous Localization and Mapping (SLAM) is the process in which a UAV can sense 
and build a map of its environment while also using this map to update its position. An 
immediate advantage to SLAM is that no a priori information is needed about the map, the 
landmarks within the map, or the vehicle location within the map. This relieves the require¬ 
ment for reference data or initial knowledge of position associated with other terrain-based 
navigation techniques listed above and allows the UAV to operate in unfamiliar areas. The 
SLAM process is discussed in [36], [37], and [38], though a brief review of its basic ele¬ 
ments is provided herein. In SLAM, the vehicle starts its navigation using its dead reck¬ 
oning sensor or vehicle model. As on-board sensors detect and extract features from the 
environment, an estimator augments the landmark locations to a map in some global ref¬ 
erence frame and begins to estimate the vehicle and map states together with successive 
observations. The estimator, which is typically an EKF, keeps track of the estimate of the 
uncertainty in both the vehicles position and the landmarks it has seen in the environment. 
The ability to estimate both the vehicle location and the map is due to the statistical cor¬ 
relations which exist within the EKF between the vehicle and landmarks and between the 
landmarks themselves. When the vehicle moves, the uncertainty pertaining to the vehicle’s 
new position is updated in the EKF. Eandmarks are then extracted from the environment 
from the vehicle’s new position. The vehicle then attempts to associate these landmarks 
to observations of landmarks it has seen previously. Re-observed landmarks are used to 
update the vehicle’s position in the EKF and the map accuracy converges to a lower limit 
which is a function of the initial vehicle uncertainty when the first landmark was observed. 
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New landmarks which were not previously seen are added to the EKF as new observations 
and the process continues. 

As stated in [37], [39] pioneered the application of SLAM in a large outdoor environment. 
The authors not only addressed computational issues of real-time operation, but also dealt 
with high-speed vehicle motion, nonfiat terrain, and dynamic clutter. Their results were 
accompanied by RTK GPS ground truth which solidified the practical accuracy of the al¬ 
gorithm, which involved closing several large loops. SLAM applications exist in a wide 
variety of domains, but the work of [38] is most relevant to this research since the authors 
were able to formulate and implement a SLAM algorithm for UAVs. At a theoretical and 
conceptual level, SLAM is often considered a solved problem and the steps in SLAM can 
be implemented using a number of different algorithms. However, there are issues remain¬ 
ing in computation, convergences, and data association that remain the main focus of the 
SLAM research community [37]. 

2.2 Probabilistic and Statistical Localization Methods 

As seen in SLAM, statistical techniques are used to provide an estimation of the posterior 
probability for the pose of the robot and for the parameters of the map. This leads to the 
discussion on a group of techniques that uses probabilistic and statistical approaches to aid 
in localization. The most common uses recursive Bayesian estimation, where an unknown 
probability density function is estimated recursively over time using a mathematical pro¬ 
cess model and incoming measurements of the environment. For simplicity, the following 
sections focus on the particle filter and the Kalman filter, since they tend to be the most 
commonly used approaches in autonomous systems. 

2.2.1 Particle Filters 

Particle filter localization, also known as Monte Carlo Localization (MCL), approximates 
the pose and orientation of a robot by using a Monte Carlo method. A set of samples 
(also called particles) are maintained where each particle represents a hypothesis of where 
the robot is located. When the robot moves, it shifts the particles based on its motion 
model to predict its new state. When the robot senses an object in the environment, each 
hypothesis is weighted according to its likelihood from the observation. The set is re¬ 
sampled according to their weights and the process continues with a posterior represented 
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by the entire partiele set. As the robot eontinues to move and sense its environment, the 
partieles eonverge towards the aetual pose of the robot as long as the motion and sensor 
models are reasonable. Unlike other loealization algorithms sueh as the Kalman filter and 
Markov loealization, MCL does not rely on parametrie distributions sinee the posteriors 
are represented by a random eolleetion of weighted partieles that approximate the desired 
distribution. As demonstrated by [40], MCL ean aeeommodate arbitrary sensor eharaeter- 
isties, motion dynamies, and noise distributions. It ean foeus and adapt to eomputational 
resourees by sampling in proportion to the posterior likelihood and eontrol the number 
of samples on-line. Finally, MCL is relatively easy to implement, although potentially 
eonstrained by eomputational limitations. Beeause of the stoehastie nature of the approxi¬ 
mation there are a few pitfalls with MCL. If the number of samples is small, for instanee, a 
robot might lose traek of its position beeause the algorithm fails to generate a sample in the 
right loeation. Also, if the robot has been moved or navigation was interrupted, there may 
be no surviving samples for the robot to use near its new pose. A more thorough analysis 
of MCL and partiele filtering ean be found in [41], [42], and [43]. 


2.2.2 Kalman Filters 

The Kalman filter is an algorithm that also operates reeursively on measurements observed 
over time and produees a state estimate by minimizing the mean squared error. A Kalman 
filter represents belief using mixtures of Gaussians that enable the filter to pursue multiple, 
distinet hypotheses, eaeh of whieh is represented by a separate Gaussian [40]. 

The filter is designed to estimate the state of a diserete-time eontrolled proeess that is gov¬ 
erned by a linear stoehastie differenee equation; however, most proeesses to be estimated 
and the measurement relationships to the proeesses are typieally nonlinear. To remedy this, 
a Kalman filter that linearizes about the eurrent mean and eovarianee is referred to as an 
Extended Kalman Filter (EKE). Similar to a Taylor series, the estimation around the eur¬ 
rent estimate ean be linearized using the partial derivatives of the proeess and measurement 
funetions to eompute estimates even if given a nonlinear relationship to the state variables. 
The distributions of the various random variables are no longer Gaussian after undergoing 
their respeetive nonlinear transformations so the EKE beeomes more of an ad hoe state 
estimator that only approximates the optimality of Bayes’ rule by linearization [44]. 
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The Kalman filter provides efficient update rules that can be shown to be optimal; however, 
this is under the assumption that the uncertainty in the robot’s position can be represented 
by a unimodal Gaussian distribution [45]. Sensor readings must also be assumed to map 
to Gaussian shaped distributions over the robot’s position. Finally, localization approaches 
using Kalman filters typically require that the starting position of the robot be known [42]. 

As shown by [44], one cycle of estimation in a Kalman filter can be divided into a propa¬ 
gation and update step. Before showing the equations in each of these steps, the following 
equations define the state x G M” of the discrete-time controlled process governed by a 
linear stochastic difference equation as follows 

x^+1 = Ak^k + B}Xk + yik (2.1) 

with a measurement z G that is 


Zk = HkXk + \k ( 2 . 2 ) 

where the n x n matrix relates the state at time step k to the state at step k+ 1. The 
nxl matrix B relates the control input u G to the state x, and the mxm matrix H in the 
measurement equation relates the state to the measurement z^. The random variables Wyt 
and Vyt represent the process and measurement noise respectively, which are assumed to be 
independent of each other, white, and are drawn from normal probability distributions: 

p(w)~A(0,e) (2.3) 

p{v)^N{0,R). (2.4) 

In the propagation cycle, the state of the system is propagated to the next time step based 
on the assumptions about the evolution of the system equations, the measured control in¬ 
puts, and the statistical description of the system noise. This relationship translates to the 
equations as follows: 


x+ =AytXfc-hBytUyt (2.5) 

P^=AkPkAl + Qk ( 2 . 6 ) 
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where and are the predieted mean and eovarianee of the state on the time step k 
before seeing the measurement. In the update eyele, the measurement from the sensor is 
processed to update the propagated estimate calculated during the previous cycle. This 
translates to the equations as follows: 


Sk = HkP+Hl+Rk (2.7) 

Kk = Pj^HlS,^ ( 2 . 8 ) 

Xk = x^ +Kk{zk-HkX^) (2.9) 

Pk=P^-KkSkKl ( 2 . 10 ) 


and where the a posteriori state estimate is computed as a linear combination of an 
a priori estimate x^ and a weighted difference between an actual measurement and a 
measurement prediction H^x^. The difference, Zk — called the measurement inno¬ 

vation or the residual on time step k, which reflects the discrepancy between the predicted 
and actual measurement. The nxn matrix Sk in Equation 2.7 is known as the measure¬ 
ment prediction covariance and the nx m matrix in Equation 2.8 is the Kalman gain 
or blending factor that minimizes the a posteriori error covariance. The justification for 
Equation 2.9 is rooted in the probability of the a priori estimate x^ conditioned on all 
prior measurements Z]^ (Bayes’ rule). The a posteriori estimate error covariance, given by 
Equation 2.10, reflects the variance of the state distribution. In other words, 

~A^(E[xfc],E[(x^-x^),(x^-x^)^]) 


= Nixk,Pk). 

After the update step, the process is repeated where the a posteriori estimate is used to 
project or predict the next a priori estimate. 

2.3 Overview of Cooperative Localization 

There have been a number of studies conducted on the accuracy of position estimation 
for groups of mobile robots using some form of CE. Results of [13] and [18] established 
the analytical assessment of the positional accuracy of multi-robot teams by deriving the 
upper bounds on the positional uncertainty of a group of homogeneous robots. However, 
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the assumption of homogeneity and the requirement that every robot eontinuously mea¬ 
sures the relative position of all other robots in the team is not applieable in a realistie 
scenario. Limitations in computational power and communications bandwidth along with 
inherent variability in sensors prohibit continuous transmission and classify the robot team 
as more heterogeneous. In [14], analytical expressions for assessing the positional accuracy 
of heterogeneous robot groups were determined as well as a study on the time evolution 
of the positional uncertainty with arbitrary topology of a Relative Position Measurement 
Graph (RPMG). Using CL, the exchange of positioning information benefits all robots 
since demonstrations have shown a smaller rate of uncertainty than the rate the single best 
agent would attain it it were localizing on its own [14]. In the same work, the authors were 
also able to show that the connectivity of the RPMG affected the constant part of the covari¬ 
ance matrix that describes the positional uncertainty of the group. A temporary reduction 
in the number of relative position measurements can only cause temporary loss of position¬ 
ing accuracy. Moreover, if one robot has absolute position measurements, the localization 
uncertainty for all robots in the group is drastically reduced. Finally, they also concluded 
that no matter how many robots were in a group, the most important factors in determin¬ 
ing an increase in position uncertainty was each robot’s own proprioceptive sensors and 
orientation estimates. 

Multi-robot systems can be classified as centralized or decentralized and distributed or non- 
distributed. The definitions of these terms are provided by [46] and [47]. It is important 
to note that a system’s characterization as centralized or decentralized is independent of its 
characterization as distributed or non-distributed. For example, a distributed system may 
have each agent perform a part of a computation while requiring a centralized processor 
to combine the computations to determine the final result. In a decentralized architecture, 
redundancy exists by allowing all other agents to proceed with their computations even if 
an agent fails. A distributed system allows for improved efficiency by making use of the 
computational resources available from multiple agents. When a system is both decentral¬ 
ized and distributed, the computation for a task does not depend on a specific agent and can 
be divided among a number of agents. 

One of the major challenges to CL is the computational complexity and the communica¬ 
tions required between agents. Research in the field started with demonstrations of simple 
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concepts and as processes evolved, work in CL began to address the more eomplex issues. 
Early work in CL was done by [48] and [49]. Kurazume et al. were the first to introduce the 
idea of using robots as features in the environment. He as well as Rekleitis in [49] started 
by having one or more robots in a group move while others remained stationary. The statie 
robots served as landmarks or acted as beaeons as the moving robots tried to loealize. The 
robots then switched roles whieh created a “leap-frogging” motion as the group moved to¬ 
wards its goal. The obvious drawbaek for this approaeh was the constraint on the motion of 
the robots. Also, eomputational and eommunication eomplexity was not addressed. Early 
variations of CL were essentially centralized systems. As computer processors began to 
get smaller, the ability to put multiple eomputers on multiple robots allowed researchers 
to begin examining how some of the eomputation for pose estimation eould be distributed. 
Sanderson [50], Madhavan [51], and Eox [43] each had success in distributing estimate eal- 
eulations but they were sub-optimal with respect to a centralized estimator. In [17] and [20], 
Roumeliotis and Bekey deeomposed the EKE into a number of filters that performed the 
prediction step loeally on eaeh robot. They then showed how the propagation of the eovari- 
anee matrix could be faetored using singular value deeomposition such that the faetored 
terms could be eomputed by eaeh robot using its own odometry data. Eor the measurement 
update, all robots in the group needed to eommunieate with each other so that the faetored 
terms eould be eombined. Their approaeh was one of the most notable advanees in CL 
beeause their results were eomparable to the performance of a eentralized estimator. 

2.3.1 Probabilistic and Statistical Methods 

The most common estimators in CL use a Kalman filter; however, there are other ap- 
proaehes that use other probabilistie and statistieal methods such as particle filters and 
Maximum Likelihood Estimations (MLEs). In [43], Eox et al. use a particle filter where 
eaeh robot maintains a probability distribution deseribing it own pose based on odometry 
and environmental sensing. This distribution is refined through the observation of the other 
robots in the group. 

In [52], Howard uses a combination of MLE and numerical optimization. A set of pose 
estimates as well as a set of observations are eolleeted for a partieular robot at a partieular 
time. There are two sets of observations where one is made by a motion deteetor and the 
other is made by a robot deteetor. Using numerical optimization, the set of pose estimates 
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that is likely to give rise to the combined set of observations is determined. The set of pose 
estimates are not used directly since the estimates are defined with respect to an arbitrary 
coordinate system whose relationship with the external environment is not defined. How¬ 
ever, the robot uses the estimates to compute the pose of every other robot relative to itself, 
and uses this information to coordinate activity. 

A distributed Maximum A Posteriori (MAP) estimator for CL is described in [53]. It is for¬ 
mulated as a nonlinear least-squares problem and solved iteratively using the Levenberg- 
Marquardt (LM) minimization algorithm. The LM algorithm is accomplished in parallel by 
all robots using a distributed conjugate gradient algorithm that provides an intermediate so¬ 
lution at every iteration. This, coupled with distributed marginalization of past robot poses, 
allows the robots to trade processing for accuracy when resources are scarce. Simulation 
results showed better performance in terms of accuracy while having lower computational 
requirements as compared to the EKF approaches, but a limitation of the distributed con¬ 
jugate gradient algorithm is the synchronous communication that is required between the 
robots. 

2.3.2 Cooperative Localization in Wireless Networks 

Although most methods to CL originate in robotics, other disciplines also rely on precise 
localization. For example, CL methods are also used in wireless networks where automatic 
localization of sensors is important for their data to be meaningful and where location in¬ 
formation can be useful for routing algorithms. In [47], [54], and [55], measurement-based 
statistical models such as angle-of-arrival, time-of-arrival, wideband and ultra-wideband 
measurements, and received-signal-strength are discussed as well as fundamental Bayesian 
and non-Bayesian CL algorithms. Methods to estimate location estimation precision, such 
as the Cramer-Rao bound, are also discussed to help select measurement technologies and 
evaluate localization algorithms. 

2.3.3 Communications and Network Challenges 

In a cooperative multi-agent system, reliable and effective communication is critical for 
enhancing the group’s performance. A common assumption in research on multi-agent 
systems is that a fully connected network is continuously available and that it is always 
possible for information from one agent to reach another. This assumption, though, is 
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not always realistic in all environments. Occlusions can obstruct and limit communication 
range and limited bandwidth may not be able to support large exchanges of data between 
all agents at once. Sequencing of information can be a challenge, which leads to the out-of¬ 
sequence measurement or negative timestep problem. Missed measurements are also pos¬ 
sible in a dynamic network. In those applications where more measurements are needed 
for better estimates, a missed measurement could cause sub-optimal performance. An¬ 
other issue when performing CL in a dynamic network is the cyclic update problem where 
agents repeatedly use the same measurement. This can cause inconsistent or over-confident 
estimates. The focus of this thesis is the demonstration of CL in UAVs. Although over¬ 
coming these communications and network challenges offer many opportunities for future 
research, they are outside the scope this work. Ways to address these issues are further 
discussed in [46]. In his work, Leung not only summarizes remedies proposed by other 
researchers, but also introduces his centralized-equivalent approach. His work avoids both 
the out-of-sequence measurement and cyclic update problems in a dynamic network and 
allows robots to recover an estimate equivalent to that produced by a centralized estimator 
whenever possible. 
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CHAPTER 3: 
Algorithm Development 


Roumeliotis and Bekey showed in [20] how to treat the Kalman filter equations of a cen¬ 
tralized system so as to distribute the pose estimation process among M Kalman filters, 
each of them operating on an agent within the group. They initially formulated the group 
localization problem in a centralized way and then described how it could be distributed 
among the agents in the group. Although the objective of this thesis is to demonstrate their 
distributed approach for 3-D motion where computations are being done by the agents, 
this chapter details an implementation of the distributed approach using a centralized node. 
Their equations and processes, detailed in [20], are applied to ground robots in 2-D using a 
unicycle motion model. The contribution of this thesis extends their work in generalizing 
equations where they presented special cases. This allows application of the equations to 
M agents and where any agent i interacts with any agent j. Another contribution includes 
incorporation of a simple aircraft motion model with lateral velocity control vice the unicy¬ 
cle model shown by Roumeliotis and Bekey. Finally, the modifications necessary to apply 
their CL algorithm in 3-D are derived and presented in this chapter. Before examining the 
2-D implementation, the following general assumptions are made for CL using an EKF: 

1. A group of M independent agents move in an A-dimensional space. The motion of 
each agent is described by its own linear or nonlinear equations of motion; 

2. Each agent carries proprioceptive and exteroceptive sensors to propagate and update 
its own position estimate; 

3. Each agent also carries exteroceptive sensors that allow it to detect and identify other 
agents moving in its vicinity and measure their respective displacement (relative po¬ 
sition and orientation); 

4. All agents are equipped with communication devices that allow exchange of infor¬ 
mation within the group. 

3.1 Two-Dimensional Approach 

Before discussing the 3-D implementation relevant to aerial applications, this section starts 
by generalizing the 2-D (N = 3) equations presented in [20] where necessary. Extending 
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previous models, lateral veloeity eontrol to the motion model of eaeh agent is also intro- 
dueed. The propagation equations for the Kalman filter using the veloeity measurements 
from the odometrie or inertial sensors are deseribed where the ehoiee of notation is con¬ 
sistent with [20]. The state vector for each agent consists of the agent’s pose with respect 
to a fixed reference frame x, = [xi yi 0,]^ where i = 1,..,M, xt and yt are the position of 
agent i in Cartesian coordinates, and 9i is the agent’s heading. A diagram visualizing this 
convention as well as showing the respective linear and angular velocities of agent i are 
shown in Figure 3.1. Note that lateral velocity (V^^) is used in the system dynamics model 
since quadrotors are being utilized. Therefore, the continuous time equations for the motion 
expressed in local coordinates are 

Ly, = vl, e, = co,m, (3.1) 

with 

~ ~ COi + Wco^ i = 1,...,M (3.2) 

where 1/^, and (0,„, are the forward linear, lateral linear, and rotational velocity of agent 
i as measured by the on-board sensors, Vf, Vj and (0/ are the real values of these quantities, 
and wv/x, Wyy and Wm, are zero-mean white Gaussian noise in the measured signals. 


y 



Figure 3.1: Visualization of fixed and relative reference frames for state representation of agent 
i within a group of UAVs 
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3.1.1 Initial Propagation and Update 

Based on Equation 3.1, the nonlinear system dynamics for propagating the state of agent i 
expressed in global coordinates is 
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Xi 
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im 
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im 
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Giim 


= Xi{tk)+Bi{tk)vii{tk) (3.4) 


for i = 1, ...,M where 5t = — h is the discrete incremental time step, x,- is the state 

vector, Bi G is the effect matrix, and u/ is the input vector which includes noise in 
the measured linear and angular velocity. In the propagation step of the EKE, the nonlinear 
system as described in Equation 3.4 is used to compute the state propagation of each agent. 
To compute the propagation of the covariance, the system dynamics expressed above must 
be linearized. Equation 3.3 is re-written as 
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and the Jacobian is computed, which results in 
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where A,- G is the linearized system propagation matrix. The linearized diserete-time 
state propagation equation is now 


or 
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(3.7) 


(3.8) 


where Gj G is the system noise input matrix, and w,- is a veetor representing the system 
noise due to the errors in the linear and rotational veloeity measurements of agent i. The 
system noise eovarianee Qi G is given by : 


Qi{tk) = Gi{tk)E{wi{tk)wf itk)}Gf {tk). (3.9) 


The eovarianee propagation for eaeh agent is therefore, 

Pit = M{h+i,tk)Pii{tk)Aj (ri+iri/t) +Qiitk)- (3.10) 

This eovarianee deseribes the uneertainty assoeiated with the position of agent i. Under the 
assumption that the motion of eaeh agent does not direetly affeet the motion of the others, 
the eovarianee propagation equation for the eentralized system is 

=^{tk+utk)P{tk)^'^{tk+utk) + Q{tk) (3.11) 

where the eentralized system matrix G system noise eovarianee matrix 

Q G eontains the propagation matrix and noise eovarianee matrix of eaeh agent. 
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respectively, on the diagonal as shown below 
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Initially, each agent may only know its own position in global coordinates and the uncer¬ 
tainty related to it. With no a priori shared knowledge among the agents, the covariance 
matrix P for the centralized system is also diagonal, and each of the diagonal elements is 
the covariance for the state of each agent. Therefore, 
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(3.12) 
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03x3 

03x3 ■ 
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While no relative position information is exchanged between agents in the group, the co- 
variance remains the same until the next propagation step. 

Next, the update step considers the measurement between the agents. When an agent can 
sense and identify another, it measures the relative pose and orientation of the agent with 
respect to the frame of reference attached to the measuring agent. The measurement is used 
to update the pose estimate for the centralized system and the covariance of the estimate. 
In [20], Roumeliotis and Bekey use an example where agent 1 detects agent 2, and then 
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uses its exteroceptive sensing to measure the relative position and orientation of agent 2 
with respect to the frame of reference attached to agent 1. Generalizing their example and 
representing the measurement when agent i detects agent j is given as: 
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1 I 
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1 1 
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Oi 




+ %■ 


(3.13) 


where 


m) 


cos(0/) -sin(0/) 
sin(0,) cos(0/) 


(3.14) 


is a rotation matrix, and n/y is a vector representing the measurement noise associated with 
the relative pose measurement between agents i and j. The measurement noise is assumed 
to be zero-mean white Gaussian with covariance 


Rij{tk) =E{nij{tk)n[j{tk)} (3.15) 

By linearizing Equation 3.13, the measurement error can be approximated by the following 
equation: 

(3.16) 

where Hij is represented as a 1 x M block matrix where each column contains dsvN xN 
sub-matrix. The zth column of the 1 x M block matrix contains a —Hij sub-matrix that rep¬ 
resents the linearized measurement with respect to agent i. The yth column of Hij contains 
the linearized measurement with respect to agent j which happens to be an x identity 
sub-matrix I^xN- All other columns contain the N x N zero sub-matrix OatxTV- This matrix 
is then multiplied by the transpose of the N x N transformation matrix E,-. In the example 
provided in [20], the H 12 matrix is therefore 

Hn = r\[-Hn hx3 Ogxs] (3.17) 
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with 


and 


ri = 


cos(0i) 

sin(0i) 

0 


— sin(0i) 0 
cos(0i) 0 
0 1 


Hn = 


’1 0 
0 1 

0 0 1 


(3.18) 


(3.19) 


To generalize the above example, H when agent i measures agent j is therefore 



1 0 
0 1 
0 0 


■( 3^1 




yi 

+ 


(3.20) 


When applying Hij in the eentralized system (as when ealeulating the eovarianee of the 
residual), it ean be used as an x {N X M) matrix. By substituting from Equation 3.12 and 
Equation 3.17, the eovarianee of the residual is ealeulated as follows: 


with 

% = «5 + Pjj + (3.22) 

The residual eovarianee S has dimensions N xN, the same as if one were updating the pose 
estimate of only one agent instead of M agents. In the latter ease, the dimension of matrix 
S would be (A^ x M) x (N X M). To eomplete the update step, the Kalman filter gain for 
this measurement is given by 


K{tk)=P+H[j{tk)Sx/{tk). (3.23) 

If K{tk) is a veetor of length M where eaeh element represents the Kalman gain for eaeh 
agent, then Ki — —P^HjjSJj^Ti and Kj = E^A^r.^E,. All other elements in K{tk) are 0 for 
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this measurement. In the example provided by [20], 
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The pose estimate for the centralized system is given by 

X=X+ + K{tk)rij{h) 


(3.24) 


(3.25) 


where Vij is the residual of the relative pose measurement as shown in [20]. Only the agents 
involved in the measurement are updated. Therefore, let X be a vector containing the state 
of all agents in the group after the update step. Equation 3.25 is rewritten as 
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(3.26) 


Applying the example from [20] yields the special case: 
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Finally, the covariance update for the centralized system is calculated as 

P = P+ -P+Hj^{tk)Si.\h)Hij{tk)P+. (3.28) 
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By substituting for from Equation 3.12 and for Htj, only the covariances of the agents 
involved in the measurement as well as their cross correlation terms change. This means 
that 


Pii=Pu-PuHJj^j'HijPu 



p+_ n+ c ln+ 

jj jj ij jj 





(3.29) 


where Pij corresponds to the ith row and jth column of the centralized covariance matrix 
P. The remaining covariances remain unchanged and their cross covariances remain as 0. 
For the example in [20], this yields 
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3.1.2 The Introduction of Cross Correlation Terms 


Note the above derivations of the covariance updates are only valid for the first iteration of 
the multi-agent CL approach. When the first update is complete, cross correlation elements 
are introduced in the covariance of the state estimate as shown in Equation 3.29. This 
matrix is now written as (c.f. Equation 3.12) 
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(3.31) 


Each agent i continues to move independently of the others and its motion is described 
by Equation 3.1. Since the measured quantities are local to agent i, the state propagation 
equations can be distributed among the agents and remain as described by Equation 3.3 and 
Equation 3.4. The measurement z from Equation 3.13 also remains unchanged. However, 
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the same is not true for the covariance. Substituting from Equation 3.31, the covariance 
propagation is given as 


Qd{h) 

AiE+A[ + ei MPti^l ... AiEVm 

A2/2lAf A2/22A2 +22 ... ^2P2M^M 


(3.32) 


Am^^2^2 • • • + 2m 


where the calculation of each of the propagated diagonal sub-matrix elements of the cen¬ 
tralized covariance matrix requires the processing of odometric or inertial measurements 
from the corresponding agent 


Pu = ^i{h+l,tk)Pii{tk)Aj {tk+\,tk) + Qi- (3.33) 


The residual covariance matrix update will be 

SiAh) = Hij{lt)P+HlAh) 

(3.34) 

= PfSijri 

with 

4- = + fji + ri^.7('t)rr (3.35) 

where Rij{tk) is the measurement noise covariance matrix associated with the relative pose 
measurement between agents i and j and is defined similarly to Equation 3.15. To calculate 
matrix Sij{ti^), only the covariance between the two agents is needed along with their cross 
correlation terms. These terms would be exchanged when the agents detect each other. The 
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Kalman gain for this update is now 
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From the example given in [20], the Kalman gain when agent 2 meets agent 3 is 
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The pose estimate update is computed as 
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and the covariance update is calculated as before by applying Equation 3.28 which gives 
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(3.41) 


where the first block row, second block row and Mth row of the covariance matrix is 
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3.2 Applying the Third Dimension 


When adding another dimension, namely altitude z, the dynamical system first needs to be 
modified with appropriate inputs. The state x,- as well as the input u,- for agent i is now 

(3.43) 

where Zi is the altitude of agent i, is the measured linear velocity in the z axis defined 
similar to and in Equation 3.2. The heading and angular velocity about the z axis re¬ 
mains defined as 9i and (Oim as in the 2-D equations. Measured angles and rates to pitch and 
roll could also be applied, but these added degrees of freedom add unnecessary complexity 
to this nonlinear model of an aerial robot. Therefore, the nonlinear system dynamics are 
now 
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To determine the linearized propagation matrix A, the Jacobian of the system dynamics is 
computed, which results in 
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The system noise input matrix G,- from Equation 3.8 as well as the effect matrix Bi from 
Equation 3.4 is now 




dt cos 6i —dt sin 6i 0 0 

5tsin0,' 5t cos 9i 0 0 

0 0 5t 0 
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(3.46) 


and the system noise covariance Qt is determined as in Equation 3.9, with the system noise 

r 1 ^ 

w,- now wv^ w^y Wv^ Wm, . The propagation of the state and covariance remains the 

L ^ im im 'J x x 

same as in Equation 3.4 and Equation 3.10, respectively, only now the covariance Pa is a 
4x4 matrix for each agent i. The measurement for the update step incorporates the z axis 
as follows 

r /r 1 

\Xi\ 








yi 


+ n 


O’ 


(3.47) 
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Bj-e, 

where C(0;) remains the same as Equation 3.14. To determine Hij, Equation 3.47 has to be 
linearized. Expanding the measurement yields 




{Xj - Xi) COS Bi + {yj - yi) sin Bi 
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+ 11,7 


(3.48) 


Since the distributed approach to CE is being used, the only concern is linearizing Zij{tk) 
with respect to the local agent i. Therefore, Hij needs to be found which incorporates 
the z axis and satisfies the relationships found in the 2-D form of Equation 3.16 through 
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Equation 3.18. Taking the Jacobian of Equation 3.48 with respect to agent i gives 
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(3.49) 


Erom the 2-D example in [20], multiplying E^ from Equation 3.18 by —Hn in Equa¬ 
tion 3.19 gives 


-cos 01 —sin 01 — (v 2 —vi)sin0i-|-(y 2 —yi)cos0i 
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0 0 -1 


(3.50) 


which matches the 2-D form of Equation 3.49 with respect to agent i, given by: 
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Since Equation 3.50 and Equation 3.51 are equal, this relationship can be used to determine 
Hij that incorporates the z axis. Define the new Fi as 
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To satisfy the relationship where Tf {—Hij) = Hij with respect to agent i, Hij is therefore, 
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(3.53) 


Equation 3.21 through Equation 3.42 can now be used to conduct CL with altitude incor¬ 
porated. 
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CHAPTER 4: 

Methodology 


4.1 Resources 

4.1.1 Research Platform 

The implementation of CL presented in this thesis starts by selecting an appropriate UAV 
that can represent an agent within a swarm while being relatively easy to program, con¬ 
trol, and conduct flight experiments. Another consideration includes compatibility where 
a common architecture can be used for communication and computation among different 
platforms. One last desire is to have a vehicle that does not have the flight restrictions that 
are placed on the larger UAVs. With these requirements in mind, the Parrot AR Drone was 
selected for this thesis. Shown in Figure 4.1, the AR Drone is a good compromise between 
the inexpensive toy helicopters, which are hard to fly and have limited capabilities, and the 
extremely expensive remote controlled helicopters built by enthusiasts and hobbyists. The 
quadrotor can be controlled by a smartphone or tablet, is intuitive to fly, can be flown in¬ 
doors, and features two built-in cameras. The quadrotor relies on an Android or iOS device 
connected to a Wi-Fi hotspot; however, the ardrone_autonomy package for ROS is used 
to provide the ability for controlling the quadrotor through a laptop computer. ROS and 
the drivers necessary to control the AR Drone through ROS are discussed in more detail in 
Section 4.1.3. More details about the AR Drone and the AR Drone Software Development 
Kit (SDK) can be found in [56]. Technical specifications of the AR Drone and more details 
about its navigation and sensor suite can be found in [57] and [58]. 

4.1.2 Vicon 

The Vicon motion capture system is used in this thesis to gather truth data and to serve as 
the upper-bound for measuring localization performance. Vicon is an Infrared (IR) marker 
tracking system that offers 3-D millimeter resolution of object displacement. It offers a high 
order of positional and angular precision by delivering positional data in all six degrees- 
of-freedom with low latency. The system used for this project consists of 10 cameras 
outfitted with IR optical filters and an array of IR LEDs. Two of these cameras are shown 
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Figure 4.1: Parrot AR Drone 1.0 


in Figure 4.2. A set of five reflective markers placed in a unique pattern on the hull of the 
AR Drone (example shown in Figure 4.3) reflect the IR radiation emitted by the LEDs. All 
other light is filtered so the system only recognizes the markers. Software is then used to 
construct the 3-D representation of the markers from the images taken by all cameras. The 
software can then uniquely identify each AR Drone by the constellation of markers on its 
hull and precisely localize it when the quadrotor is within field of view of the cameras. 
There are a number of different software programs for Vicon but for this project, Vicon 
Tracker 1.3 is utilized. A screen shot of the Graphical User Interface (GUI) showing three 
AR Drones being identified and tracked is shown in Figure 4.4. More details about Vicon 
and its technical specifications are found in [59]. 

4.1.3 Robot Operating System 

ROS is a flexible framework for developing and executing robot software. It is a collection 
of tools, libraries, and conventions that simplify the creation of robot behavior across a 
wide variety of platforms. One of its primary goals is to provide an open-ended collabora¬ 
tion framework for those in the robotics research community and it was developed in the 
open using the permissive Berkeley Software Distribution (BSD) open-source license. One 
of the core components of ROS that is relied on heavily in this project is its communication 
infrastructure. A node in ROS is simply any process that performs a computation. Nodes 
are combined together into a graph and communicate with one another using unidirectional, 
streaming topics. Topics have anonymous publish and subscribe semantics, which means 
nodes are not aware of who they are communicating with. Instead, nodes that are interested 
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Figure 4.2: Two of the ten Vicon motion capture cameras used to capture true position and 
orientation of the AR Drones used in this project. 

in data subscribe to the relevant topic and nodes that generate data publish to the relevant 
topic. ROS also provides recording and playback of messages, request/response remote 
procedure calls, and a distributed parameter system. With this simple construct, ROS al¬ 
lows for the visualizing and recording of flight parameters, integration of other supporting 
tools for the experiment (i.e., Vicon), and for the ability to control multiple AR Drones 
on one centralized system. To be able to interface with multiple AR Drones and the Vi¬ 
con system, the ardrone.autonomy and vicon_bridge packages provided for ROS are 
used. The ardrone.autonomy package not only provides the interface between ROS and 
the AR Drone, but also allows for more functionality and direct control of features that 
are suppressed by the stock applications provided for the quadrotor. The vicon_bridge 
package allows pose information in all six degrees of freedom for each agent to be passed 
into ROS from Vicon as a topic. More details about these packages can be found in [60] 
and [61], respectively. More information about ROS can be found in [62]. 
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Figure 4.3: IR reflective markers placed on the hull ofthe AR Drone are used by the Vicon system 
for identifying and accurately tracking the quadrotor. 


4.2 The Experimental Environment 

4.2.1 Executing the CL Algorithm 

The 2-D distributed CL algorithm for a unieyele model as shown by [20] was first eoded 
and tested in Python 3.3.2. The numpy and matplotlib paekages for Python were used to 
eomplete the matrix operations as well as visualize the results of the pose estimation. Then, 
the lateral veloeity eontrol inputs (V^^) were added to the dynamies model and appropriate 
ehanges to the affeeted veetors and matriees were made. Onee the 2-D algorithm was fune- 
tioning properly with no known semantie errors, the 3-D modifieations from Seetion 3.2 
were made using the applieable dynamies model and tested in the simulated environment. 
Figure 4.5 shows an example of the simulation results where three agents are eondueting 
CL using the 2-D unieyele model. Eaeh agent is assigned a eonstant forward linear veloeity 
with no lateral movement or heading ehange. Agent 1, shown in blue, is at the top of the 
plot and is assigned to move at a heading of 45 degrees where 0 degrees is along the pos¬ 
itive X axis. Agent 2, shown in red, is heading at 0 degrees and Agent 3, shown in green, 
is heading at -45 degrees. The plot marked by an x is the traek of the agent’s dynamie 
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Figure 4.4: Screen capture of the Vicon Tracker 1.3 software displaying real time identification 
and tracking of the quadrotors used in this project. 


model if there was no noise in the veloeity measurement. The plot marked by a thin line is 
the agent’s actual movement with noise introduced in the velocity input. Finally, the dotted 
line is the pose estimation of the agent using the CL algorithm where one measurement is 
made in each time step. The relative pose measurement is continuous and rotates between 
Agent 2 measuring the pose of Agent 3, and Agent 3 measuring Agent 1. 

4.2.2 Pose Estimation and Control of the AR Drone 

To examine the performance of the CL algorithm, another method of pose estimation is 
required for comparison. Because the AR Drone has no native means to estimate its po¬ 
sition, a pose estimator using DR was created in ROS. A node called est_state_imu 
was written for each agent in the swarm. When the node is run, it takes the initial pose 
of the agent by receiving data from Vicon. At each time step, the node takes the linear 
and angular velocity measurements for each axis from the agent’s on-board IMU. To get 
all required velocity measurements, the node has to subscribe to both the ardrone/imu 
and ardrone/navdata topics, which are included in the ardrone.autonomy ROS pack- 
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Figure 4.5: Three agents conducting CL in Python using the 2-D unicycle model found in [20]. 
Each agent is assigned a constant forward linear velocity with no heading change. Relative 
pose measurements are continuous where Agent 2 measures the pose of Agent 3, and Agent 3 
measures Agent 1 on the subsequent time step. 


age. At each time step, the est_state_imu node multiplies the measured velocity by the 
time step and adds the resulting displacement to the previous pose estimate. The resulting 
pose information including x, y, z, pitch, roll, and yaw in Euler angles is published via a 
topic called estPose/imu. In the same message, the linear and angular velocities from the 
quadrotor’s IMU are echoed to consolidate the information which was extracted from two 
separate topics. This helps streamline and synchronize measurements. 
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To ensure the measured velocity from the IMU on the AR Drone was accurate, a com¬ 
parison test with Vicon was completed. A single AR Drone was commanded by a simple 
controller to fly forward in its x axis at a constant velocity and altitude for a designated 
period of time. No commands to turn about the z axis or fly laterally were given. Vicon 
was used to track and record actual displacement of the quadrotor during its flight. Using 
the displacements and knowing the time between pose measurements made by Vicon (ap¬ 
proximately 0.01 seconds), the actual velocity of the quadrotor was calculated and plotted 
for this single degree-of-freedom. Velocities reported by the AR Drone’s IMU were also 
recorded and plotted. This process was repeated for commanded linear velocities of 0.1, 
0.2, 0.5, 0.8. and 1.0. Rearward velocities (i.e., negative commanded velocity in the x axis) 
at the settings specified previously were also recorded. When data for the forward and 
rearward velocity were collected, the test was repeated for the quadrotor’s lateral velocity 
and for angular velocity about the z axis. The same quadrotor was used for all tests and 
its configuration was kept constant. Calculations did not account for any test day condi¬ 
tions such as drift or any aerodynamic cross-coupling which in the context of this project 
were not significant to the results. The observed data showed that the velocities measured 
and reported by the AR Drone’s IMU were comparable and for the most part within 0.3 
meters per second (m/s) to the actual velocities calculated by the reported Vicon positions. 
The standard deviation was also estimated when the constant velocities were observed. All 
standard deviations from all velocity tests where then averaged to determine the value used 
for the standard deviation in the velocity measurements for the CL algorithm. Figure 4.6 
and Figure 4.7 show two of the plots from the collected data. Each plot shows the linear 
velocity in the x axis as calculated from the Vicon data and the velocity reported by the AR 
Drone IMU. The figures show a 0.1 and 1.0 commanded linear velocity respectively. Note 
that noise in the Vicon measurement required a low pass filter for smoothing the calculated 
velocity. 

From [60], it was noted that linear command velocities are in the range from -1.0 to 1.0. 
These settings do not represent the actual velocity being commanded from the AR Drone, 
unlike the commanded angular velocities, and is significantly lower than what was actually 
observed in the above tests. For building controllers that can accurately maintain a constant 
velocity in the AR Drone, it was necessary to map the commanded velocity to the actual 
velocity observed at that setting. Using the forward linear velocity data collected from 
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Figure 4.6: The forward linear velocity measured by Vicon and the forward velocity reported by 
the AR Drone IMU when a constant commanded velocity of 0.1 is sent to the quadrotor. 

the above tests, Figure 4.8 shows the command velocity setting versus the sustained maxi¬ 
mum velocity observed. A curve fit was applied to the points and the resulting function to 
represent the curve was estimated as 


^ Vx cmd vel = 0.1 



0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 

Time (sec) 




(4.1) 


where Vd is the desired velocity, Vmax is the maximum observed velocity of the AR Drone 
(1.4 m/s), Vcmd is the command velocity sent to the quadrotor, and 7 is a constant set to 5.5. 
Therefore to command a specific velocity, one solves for Vcmd to get: 


In 


^cmd 


f-^) 


■7 


(4.2) 
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Figure 4.7: The forward linear velocity measured by Vicon and the forward velocity reported by 
the AR Drone IMU when a constant commanded velocity of 1.0 is sent to the quadrotor. 


4.2.3 Controlling Multiple Quadrotors with a Single ROS Master 

Additional processes are needed to develop a centralized arehitecture for eontrol of multi¬ 
ple AR Drones. This centralized eontrol eases data collection and analysis, safety of flight, 
eommunication, and management of the numerous ROS nodes required for the experiment. 
First, eode was developed that scans WiFi for AR Drones attempting to eommunieate with 
other wireless deviees when they power-up. When a quadrotor is found, it is assigned a 
unique Internet Protoeol (IP) address based on its network id and brought into a eentralized 
network. This network contains the single ROS master as well as the network interface 
with Vicon. Another program written in Python ealled SwarmLoad.py scans for active 
AR Drones by pinging all IP addresses assigned to the quadrotors by the scanner. When 
it receives a response from a quadrotor, the program creates a ROS namespaee for the 
AR Drone, and launehes the driver on the network using its assigned IP address. These 
drivers serve as the eontrolling interfaee between the AR Drone and ROS. Sinee the same 
driver is launched for eaeh aetive AR Drone, the ROS namespaee prevents naming confliets 
and ensures eaeh resulting topic has a unique name. Finally, the SwarmLoad.py program 
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Figure 4.8: Constant commanded forward velocity setting from the ardrone_autonomy ROS 
package versus the observed maximum sustained forward velocity. The fitted curve represents 
the mapping between the command velocity and the constant maximum velocity achieved for 
that setting by the AR Drone. 


maintains a list of quadrotors that are actively on the network and publishes this list via 
the DroneList topic. The list of AR Drones used in the project along with their ROS 
namespace, wireless id, and unique network address are shown in Table 4.1. To send basic 
commands to all quadrotors on the active list another Python program called SwarmGUI. py 
is used. This program provides a GUI for sending take-off, land, emergency, and reset com¬ 
mands to selected quadrotors or all quadrotors on the DroneList. The GUI also provides 
a display for the current state and remaining battery life of each AR Drone. 

Higher level behavior such as path following, loiter, obstacle avoidance, and formation 
control can now be written in separate ROS nodes and run. There is one controller for 
each quadrotor. The node sends its agent velocity commands in all six degrees-of-freedom 
via the cmd_vel ROS topic. For this experiment, a simple waypoint controller was built 
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Side Number 

ROS namespace 

Wireless ID 

Assigned IP Address 

01 

Quadl 

ardrone_I97467 

192.I68.0.10I 

02 

Quad2 

ardrone_I98504 

I92.I68.0.102 

03 

Quads 

ardrone_258674 

I92.I68.0.I03 

04 

Quad4 

ardrone_256959 

I92.I68.0.I04 

05 

Quads 

ardrone_266IlI 

I92.I68.0.I05 

06 

Quad6 

ardrone_266678 

I92.168.0.I06 

07 

QuadV 

ardrone_I98307 

I92.I68.0.I07 

08 

Quads 

ardrone_I50853 

I92.I68.0.I08 


Table 4.1: Identifiers for Each AR Drone in the Swarm 


that uses a basic proportional-integral-derivative controller (PID). Agents receive pose data 
through either the est_state_imu node via the estPose/imu topic or from Vicon via the 
vicon/ARSENL_QUADm/ARSENL_QUADm topic where m is the quadrotor’s id number. For¬ 
ward velocity, track error, altitude, and heading error are determined based on the agent’s 
current pose and the goal waypoint. A gain is then applied to each of the errors and a 
resulting velocity is sent to the agent to correct for the error. When an agent reaches the 
goal waypoint, another waypoint is assigned. This process is repeated until the agent is 
commanded to do otherwise by the SwarmGUI. Four waypoints were created that, when 
connected, form a three by three meter square where the center of the square is the ori¬ 
gin of the Vicon reference area. Each quadrotor is assigned to fly separately to each of 
the four waypoints and then repeat the cycle. Since there is no collision avoidance built- 
in to the controller or in any other node, each AR Drone is initially assigned a different 
starting waypoint. Upon reaching their assigned waypoint, the next point is assigned in a 
counter-clockwise fashion. More details about the ardrone.scanner. py, SwarmLoad. py, 
SwarmGUI. py, and waypoint controller programs can be found in the source code which is 
located at http://faculty.nps.edu/thchung under Software Resources. 

4.2.4 The Relative Pose Measurement Graph 

To measure relative pose among the agents within the swarm, another node in ROS called 
est_relDist is created. Although ultimately, the sensors that are native to the AR Drone 
(i.e., the video cameras) would be used to help identify and measure the relative pose of 
agents in the swarm, Vicon is used in this experiment. It provides a more precise means 
to measure the position of each agent and allows better control of the noise in these mea- 
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surements when evaluating the performanee of the CL implementation. The est_relDist 
node first determines whieh AR Drones are aetively on the network by subseribing to the 
DroneList. An objeet is ereated for each agent which then subscribes to the pose infor¬ 
mation for that agent from Vicon. The difference in x, y, z, pitch, roll, and heading (0) 
expressed in local coordinates are taken between each agent and can be represented by an 
RPMG. The RPMG can be shown by an M x M block matrix as follows: 



Agent 1 

Agent 2 

Agent 3 

Agent M 

Agent 1 

06x1 

X2-X1 

X3-X1 

... Xm - Xl 

Agent 2 

xi -X2 

06x 1 

X3-X2 

... Xm - X2 

Agent 3 

Xl -X3 

X2-X3 

06x1 

... Xm - X3 

Agent M 

Xi -Xm 

X2-XM 

X3-XM 

06x1 


where x G is the vector representing the pose of each agent as defined in Section 3.1 
with pitch and roll angles added, and M is the number of agents in the swarm. Although 
pitch and roll angles are not used in the dynamics model for this experiment, it was written 
into this node for future use in other projects as necessary. The rows and columns of the 
matrix represent each of the agents while their intersection is the difference vector between 
the two agents’ poses. The diagonal is the zero vector. From Equation 3.47, the x and y 
differences were multiplied by the rotation matrix C^(Oi). Because the measurement of 0; 
for each agent is in this node, the multiplication with the rotation matrix for each relative 
measurement is completed before publishing the RPMG to a ROS topic called RPMG. The 
frequency in which all measurements are made can be varied but is set to 50 hertz (Hz) in 
this experiment. Although all measurements are made in one cycle, the user can determine 
if all measurements or random ones selected by the program will be published. Noise may 
also be added to the measurements and can be adjusted by the user before the program is 
run. 

4.2.5 Implementing CL in ROS 

Similar to Section 4.2.1, the EKE implementation of CE was coded in Python and then 
modified to operate in the ROS environment. The program is written to accept multiple 
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AR Drones by subscribing to the DroneList topic and then creating an object for each 
agent in the list. The object contains the agent’s state as well as all applicable matrices 
used in the EKF. The program can estimate agent pose in the 2-D and 3-D plane based on 
user input. Initial pose information for each agent is received from the respective Vicon or 
estPose/imu topic which is also set by the user prior to launching the node. Each agent’s 
velocity measurements are updated by subscribing to the applicableestPose/imu topic 
and relative pose measurements are received by subscribing to the RPMG topic. When the 
estimate step of the EKE cycle is complete, the pose estimate of each agent is published via 
their own estPose/cl topic. Although pose messages in ROS normally contain rotations 
in quaternions, the rotations in Euler angles are published and the fourth-dimension is used 
to publish the trace of the covariance matrix for the applicable agent. The frequency of a 
single cycle is set to 10 Hz but can be varied by the user before running the program. A 
summary of the experimental set-up in ROS is shown in Figure 4.9. Associated code can 
be found at http://faculty .nps. edu/thchung under Software Resources. 

4.3 Experimental Method 

The lab environment for the experiment is established, and the CE algorithm is ready to 
be implemented and its performance observed. Experiments conducted in [20] observed 
the performance of the CE algorithm when agents have and did not have access to absolute 
positioning information (e.g., using GPS or a map of the environment). Their goal was 
to demonstrate how the CE algorithm could process and distribute relative and absolute 
positioning information across a group of agents. They also conducted experiments where 
agents had intermittent relative pose measurements to show that even sparse measurements 
could effectively reduce the rate of increase in the position uncertainty over having no 
measurements at all. For this experiment, the CE algorithm for 3-D is first implemented 
using various measures of RPMG connectedness to validate the expected rate of position 
uncertainty as the connectedness of the RPMG grows. Tests begin where no relative mea¬ 
surements are made between agents. Then, the same flight data is used to execute the CE 
algorithm were only one agent measures another. Finally, CE with a strongly connected and 
fully connected RPMG is demonstrated. To demonstrate the consistency of performance, 
the algorithm is applied to multiple flights using a fully connected RPMG. 
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Figure 4.9: AR Drone Cooperative Localization ROS Architecture 


For the experiments, all programs and nodes as deseribed in the above seetions are run with 
Quad2, Quads, and QuadV. The est_state_centCL node is run prior to eommanding 
takeoff and continues to provide pose estimates while the quadrotors fly to their designated 
waypoints at a commanded airspeed of 0.2 m/s and an altitude of 1.5 m. To prevent tra¬ 
jectories from being cluttered during data plotting, each quad flies to each waypoint once 
before being commanded to land. The algorithm receives the initial pose of all agents via 
Vicon and publishes its estimates at a rate of 10 Hz. 

To represent the system noise W/ and initialize the noise covariance matrix Qi{tk), Equa¬ 
tion 3.9 is revised as 

Qiitk) = Giitk)D,Gjitk) (4.3) 
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where D,- is a 4 x 4 diagonal matrix in the 3-D model as follows 


0 0 0 
0 Oyy 0 0 

0 0 Oyz 0 

0 0 0 ctJ, 


(4.4) 


and Gyy, Gyz, and Cq),. are the standard deviation of error in the respeetive linear and 
rotational veloeity of agent i. For the initialization of measurement noise Rij{tk), Equa¬ 
tion 3.15 in the 3-D model is rewritten as 


^sensor^NxN (4.5) 

where Gsensor is the standard deviation of error for the relative position measurement of 
agent i in eaeh axis. Relative pose measurements between all agents are eomputed making 
the direeted RPMG fully eonneeted but ean be adjusted by the est_state_centCL node. 
There is no noise added to the measurements and the resulting RPMG is published at 50 
Hz. 

A total of five flights are exeeuted using the same three quadrotors starting from the same 
loeation. All assoeiated topies are reeorded and eonverted to a eomma-separated value file 
format for later analysis using MatLab. Speeifieally, the Vieon trajeetory as well as the 
DR and CL position estimates of eaeh quadrotor are plotted on a 3-D line plot in MatLab. 
Performanee of the CL algorithm is eompared against the DR pose estimate by ealeulating 
the absolute error of both estimates where pose data from Vieon serves as the truth. The 
differenee between the estimate and aetual position of eaeh axis is taken and squared. These 
values are then added and the square root of the resulting sum is made. This results in the 
following equation, 

err = sj{xa -XiY -h (y^ -y,-)^ + {za-ZiY (4.6) 

where err is the absolute error for eaeh time step, Xi, yt, Zi is the position estimate in eaeh 
axis, and Xa, ya, Za is the position reported by Vieon. The traee of the eovarianee for eaeh 
agent, whieh represents the uneertainty in the Kalman filter’s estimate, is also plotted to 
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monitor performance. Successful implementation of CL is defined as the absolute error 
and error rate remaining less than the error of the DR pose estimate for the respective 
agent. Quantitative results using these metrics are provided in the next chapter. 
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CHAPTER 5: 
Results and Analysis 


The CL algorithm for 3-D was implemented and tested for three AR Drones starting from 
different locations and flying within the same area as described in Section 4.3. Relative 
pose measurements were taken using information from Vicon. All o values of matrix D 
from Equation 4.4 were set to 0.045, and all Osensor values of Rij{tk) from Equation 4.5 
were set to 10^^ m. This small value ensures there is very little noise associated with the 
position measurement. Einally, the covariance matrix Pa for each agent was initialized to 
0.2 along the diagonal. The CE algorithm was run just prior to takeoff and test data was 
recorded for approximately 60 sec. An example of the Vicon trajectory for each AR Drone 
is shown in Eigure 5.1 from a 3-D, birds-eye (i.e., X-Y view), and eye-level view of the 
flight area. 

5.1 Varied Connectedness of the RPMG 

5.1.1 No Communication between the Agents 

The performance of the CE algorithm is first examined when no relative pose measure¬ 
ments are made between agents. Since there is no exchange of information, each agent 
dead reckons its pose estimate independently using the output of the propagation step as 
the pose estimate. The covariance remains the same until the next propagation step. Eig¬ 
ure 5.2 shows the Vicon trajectory of each agent as well as the CE and DR estimates from 
an X-Y view. Eigure 5.3 shows the resulting error, and Eigure 5.4 plots the trace of the 
covariance for each agent. In Eigure 5.4, the uncertainty for all three quadrotors increases 
over time except for Quad 2, which peaks at approximately 3 m then begins to decrease 
about halfway through the flight. It was expected that the uncertainty in all three quadrotors 
would be unbounded and increase over time; however. Quad 2 does not react this way. This 
observation is not fully understood, and requires more analysis in future work. The exami¬ 
nation should start by looking at the covariance at each time step and determine if negative 
values are introduced by elements in the propagation matrix A or the noise covariance Q. 
When the matrix causing this decrease is determined, examine each of the variables that 
make up the individual elements of the matrix. Erom Eigure 5.2, the CE pose estimate 
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for all three quadrotors is similar in shape and in the general location of the DR estimate. 
This is consistent since the CL algorithm is only using the velocity measurements of the 
quadrotor’s IMU to generate its pose estimate. 



(a) 3-D View 




(b) X-Y View (c) Eye-level View 

Figure 5.1: Position plot of all three quadrotors as recorded from Vicon 
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(a) Quad 2 




(b) Quad 3 (c) Quad 7 

Figure 5.2: Position plot of all quadrotors conducting CL where no relative pose measurements 
are made between agents. The plot includes the trajectory as recorded by Vicon as well as the 
CL and DR pose estimates for all agents in an X-Y View. 
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Quad 7: error (m) Quad 3: error (m) Quad 2: error (m) 





Figure 5.3: CL and DR pose errors of all three agents using CL with no communication between 
the agents. 
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Figure 5.4: Covariance trace of all three agents using CL with no communication between the 
agents. The covariance trace represents the uncertainty in the Kalman filter’s pose estimate. 


5.1.2 Relative Pose Measurements by a Single Agent 

For this case, Quad 2 takes eontinuous relative pose measurements of Quad 3 while Quad 7 
reeeives no information. Figure 5.5 shows the resulting error, and Figure 5.6 plots the traee 
of the eovarianee for eaeh agent. After takeoff. Quad 2 CL pose error inereases slightly 
over the DR estimate, however, it remains between 1.0 and 1.5 m while the DR estimate 
eontinues to grow and peak at approximately 3 m. This is an improvement over the pose 
estimates when there was no eommunieation between agents. Quad 3 also benefits from 
the eommunieation early in the flight but the CL pose estimate error grows approximately 
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Quad 7: error (m) Quad 3: error (m) Quad 2: error (m) 





Figure 5.5: CL and DR pose errors plotted using CL when Quad 2 only measures relative pose 
of Quad 3 continuously. 
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Figure 5.6: Covariance of all agents when Quad 2 only measures relative pose of Quad 3 contin¬ 
uously. 


the same as the DR estimate error 17 see after the start of the flight. When the agents start 
CL, the eovariance trace of Quad 2 and Quad 3 drop from approximately 1.0 m in Fig¬ 
ure 5.4 to 0.4 m in Figure 5.6. Both covariances then drop to 0.3 m when the UAVs takeoff 
and then climb steadily at a rate of approximately 0.4 millimeters per second (mm/s) for the 
rest of the flight. It was noted in [20], that when agents are moving slowly or standing still 
while continuously measuring their relative pose, there is almost no new information re¬ 
garding pose uncertainty. These repeated measurements trigger updates that only suppress 
the uncertainty associated with the relative pose measurement and not with the agents’ pose 
estimates. Therefore, when Quad 2 and Quad 3 takeoff, the measurements are no longer re- 
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peated and the calculation of uncertainty improves. It is presumed that the slight but steady 
increase in uncertainty after takeoff results from the Quads having no access to absolute 
positioning. The agents improve their position tracking accuracy by measuring their rela¬ 
tive positions, but they are not able to bound the overall uncertainty since no one has access 
to absolute positioning [20]. Quad 7 performance remains unchanged which is expected 
since it is not receiving any information being shared between Quad 2 and Quad 3. 


5.1.3 Agents Communicate over a Strongly Connected RPMG 

For this test, Agent 2 takes continuous relative pose measurements of Agent 3, Agent 3 
measures Agent 7, and Agent 7 measures Agent 2 to form a strongly connected graph as 
shown in Figure 5.7. 



Figure 5.7: Strongly Connected Graph Between Agents 


Figure 5.8 shows the resulting error, and Figure 5.9 plots the trace of the covariance for 
each agent. The covariance for all agents is 0.127 m after the first update and rises at a rate 
of 0.2 mm/s to 0.138 m before the flight ends. The error in the CL pose estimate for each 
agent is reduced as compared to the errors shown in Figure 5.5. For Quad 2, the average 
DR error is 1.89 m while the average error for the CL estimate is 0.57 m. The average DR 
and CL error for Quad 3 is 1.13 m and 0.67 m respectively, while the average DR and CL 
estimate error for Quad 7 is 1.39 m and 0.66 m. 
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Quad 7: error (m) Quad 3: error (m) Quad 2: error (m) 





Figure 5.8: CL and DR pose errors plotted using CL with a strongly connected RPMG and 
continuous relative pose measurements. 
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Quad 7: covariance (m) Quad 3: covariance (m) Quad 2: covariance (m) 





Figure 5.9: Covariance of all agents conducting CL with a strongly connected RPMG and con¬ 
tinuous relative pose measurements. 
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5.1.4 Agents Communicate over a Fully Connected RPMG 

Here, each agent continuously measures the other agents in the group to form a fully con¬ 
nected graph as shown in Figure 5.10. Figures 5.11, 5.12, and 5.13 are plots of the Vicon 
trajectory as well as the CL and DR estimates for Quad 2, 3, and 7 respectively. Figure 5.14 
shows the resulting error, and Figure 5.15 plots the trace of the covariance for each agent. 



Figure 5.10: Fully Connected RPMG Between Agents 


The covariance for all agents is 0.127 m after the first update and rises at a rate of 0.2 
mm/s to 0.137 m before the flight ends. The error in the CL pose estimate for each agent is 
further reduced as compared to the errors shown in Figure 5.8. For Quad 2 and Quad 3, the 
average error for the CL estimate is 0.50 m. The average CL error for Quad 7 is 0.60 m. A 
summary of results from the various connections of the RPMG is shown in Table 5.1. 
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X (meters) 


(a) X-Y View 




y (meters) 


X (meters) 


(b) 3-D View (e) Eye-level View 

Figure 5.11: Position plot of Quad 2 conducting CL with a fully connected RPMG and continuous 
relative pose measurments between agents. The plot includes the trajectory as recorded by Vicon 
as well as the CL and DR pose estimates. 
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X (meters) 


(a) X-Y View 




(c) Eye-level View 


Figure 5.12: Position plot of Quad 3 conducting CL with a fully connected RPMG and continuous 
relative pose measurments between agents. The plot includes the trajectory as recorded by Vicon 
as well as the CL and DR pose estimates. 
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X (meters) 

(a) X-Y View 




(b) 3-D View (e) Eye-level View 

Figure 5.13: Position plot of Quad 7 conducting CL with a fully connected RPMG and continuous 
relative pose measurments between agents. The plot includes the trajectory as recorded by Vicon 
as well as the CL and DR pose estimates. 
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Quad 7: error (m) Quad 3: error (m) Quad 2: error (m) 





Figure 5.14: CL and DR pose errors plotted using CL with a fully connected RPMG and contin¬ 
uous relative pose measurements. 
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Quad 7: covariance (m) Quad 3: covariance (m) Quad 2: covariance (m) 





Figure 5.15: Covariance of all agents conducting CL with a fully connected RPMG and continuous 
relative pose measurements. 
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Max 

Avg 

Min 

Avg Uncertainty 

Connectivity 

Quad ID 

Error (m) 

Error (m) 

Uncertainty (m) 

Rate (mm/s) 


Quad 2 

1.48 

1.11 

0.302 

0.4 

Single Measurement 

Quad 3 

1.63 

1.05 

0.302 

0.4 


Quad 7 

1.88 

0.89 

0.802 

102.9 


Quad 2 

1.40 

0.57 

0.127 

0.2 

Strong 

Quad 3 

1.40 

0.67 

0.127 

0.2 


Quad 7 

1.26 

0.66 

0.127 

0.2 


Quad 2 

1.14 

0.5 

0.127 

0.2 

Full 

Quad 3 

0.91 

0.5 

0.127 

0.2 


Quad 7 

1.10 

0.6 

0.127 

0.2 


Table 5.1: Localization performance summary of each agent conducting CL with varied connect¬ 
edness of the RPMG. Note the average uncertainty rate is in mm/s due to the small resulting 
values. 


5.2 Continuous Pose Measurements and a Fully Connected 
RPMG 

For this test, consistency of algorithm performance is demonstrated by applying CL with a 
fully connected RPMG to multiple flights. The algorithm was run on four additional flights 
using the same AR Drones starting from the same location. The error and covariance plots 
for one of these flights are shown in Figures 5.16 and 5.17 respectively. Generally, the same 
average errors and uncertainty in position estimates were observed, however one flight 
showed a significant departure in performance. Figure 5.18 shows the Vicon trajectory of 
each agent as well as the CL and DR estimates at an X-Y view for this flight. The error and 
covariance plots are shown in Figures 5.19 and 5.20. When reviewing the recorded data, 
the measured velocities were either very noisy (as indicated by the erratic and unpredictable 
spikes in the velocity data) or ROS messages that contained the velocity data were either 
delayed or not being published to the estPose/imu node. 


71 



Quad 7: error (m) Quad 3: error (m) Quad 2: error (m) 





Figure 5.16: CL and DR pose errors of all three agents using CL with a fully connected RPMG 
and continuous relative pose measurements during the second flight. 
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Figure 5.17: Agent covariance of all three agents using CL with a fully connected RPMG and 
continuous relative pose measurements during the second flight. 
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(b) Quad 3 (c) Quad 7 

Figure 5.18: Position plot of all quadrotors from the fourth flight of CL tests in an X-Y view. 
The plot includes the trajectory as recorded by Vicon as well as the CL and DR pose estimates 
with a fully connected RPMG. 
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Figure 5.19: CL and DR pose errors of all three agents using CL with a fully connected RPMG 
and continuous relative pose measurements during the fourth flight. 
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Figure 5.20: Agent covariance of all three agents using CL with a fully connected RPMG and 
continuous relative pose measurements during the fourth flight. 
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CHAPTER 6: 

Conclusions and Recommendations 


6.1 Conclusions 

The purpose of this thesis is to explore the use of Cooperative Localization (CL) in a group 
of UAVs by modifying and implementing an Extended Kalman Filter (EKE) where the 
agents’ on-board IMU provides velocity measurements for the propagation step, and rela¬ 
tive position measurements from agents with the swarm are used for the update step. Using 
the distributed approach presented in [20], the special cases presented are first generalized 
as needed so that the equations can be applied to all agents within the group as well as 
all relative pose measurements between the agents. The motion model presented is also re¬ 
placed with an aerial vehicle model which includes lateral velocity (for quadrotors), vertical 
velocity, and altitude. Upon modifying the EKE approach to CE, correct operation of the 
algorithm is demonstrated in a centralized system using AR Drones and ROS. The uncer¬ 
tainty in measurements react consistently with the connectedness of the RPMG and overall 
this approach provides a better estimate of pose over using dead reckoning. However, the 
performance of the algorithm does not provide the expected results after review of the per¬ 
formance data from [20]. The accuracy of the IMU on-board the AR Drone is presumed to 
be overestimated, and the characterization of noise in the velocity measurements may be 
inaccurate. It is also predicted that significant noise from the IMU as well as delayed or 
missing velocity messages from the quadrotors also contribute to a poor prediction of the 
agent state in the propagation step of the Kalman filter. The AR Drone is used as a low-cost 
solution to show proof-of-concept and it did aid in demonstrating correct operation of the 
proposed CE algorithm. Use of a better IMU may have provided better results. 

6.2 Recommendations 

The EKE approach to CE as described in this thesis is possible for use by swarm UAVs and 
is a recommended approach for enhancing positional accuracy of agents within a group. 
This approach not only has the potential to enhance the positional accuracy over use of 
a standard GPS but can also provide accurate positional information when encountering 
an intermittent or denied GPS environment. Further study into the application of this ap- 
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proach with use in formation control and in conjunction with other absolute positioning 
sensors such as GPS is recommended. Areas of focus should center on the challenges 
associated with computational resources on-board the UAV and communication between 
agents. The AR Drone is a great low-cost option to conduct swarm research; however, 
the platform’s performance is limited. For experimentation with higher level autonomous 
features that require more computational power and higher precision from its navigational 
sensors, modification to the AR Drone or use of more capable platforms may be required. 
These modifications are possible and have been shown in [63]. Further examination into 
possible modifications to the AR Drone that further build upon its strengths for the research 
community is recommended. 

6.3 Future Work 

In this thesis, the distributed approach to CL using an EKF is implemented in a centralized 
way. It may be beneficial to code, test, and implement the centralized approach as described 
in [20]. This would help validate the approach made by Roumeliotis and Bekey, and ensure 
the performance observed in this implementation is not due to any errors in their processes. 
Future work would also involve exploring and re-evaluating the performance of the IMU 
on-board the AR Drone. Re-examining the accuracy of the IMU and determining a more 
precise model for noise in the sensor may help determine if the CL approach in this thesis 
can be “tuned” for optimal performance or if a better IMU is required. 

In this thesis, the performance of the CL algorithm with continuous relative pose measure¬ 
ments is examined. To further observe the power of CL, it may be beneficial to run this 
implementation of CL when a random number of measurements are made between agents 
(frequency), when measurements are randomly spread over time (intermittency), and when 
more noise is added to the measurements (robustness). It is also proposed to introduce ab¬ 
solute positioning to verify the algorithm distributes the absolute position across the group. 
An extension of this test is to allow only one agent within a group to receive initial pose 
information when CL starts. A natural extension of this work is to add more agents to 
the swarm to ensure scalability and to examine the absolute communication requirements 
to conduct our CL algorithm. It would also benefit the future implementation of this al¬ 
gorithm by examining a means to identify and measure relative position between agents 
using on-board sensors (e.g., the video cameras) rather than relying on Vicon. Finally, the 
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AR Drone has limited to no on-board proeessing power for programming additional au¬ 
tonomous features and functions. In this project, CL is demonstrated in a centralized way. 
Future work should include re-creating Roumeliotis and Bekey’s work by implementing 
their decentralized approach. 

Results of this work have established a framework and associated infrastructure in which 
other CL algorithms may be implemented for the AR Drone or other quadrotors. A single 
node for ROS that contains the algorithm as well as any additional environmental mea¬ 
surements needed for its operation are the only requirements. The stage is therefore set 
for implementing and examining other CL techniques. When a suitable approach is deter¬ 
mined, future work should also involve examining its use in autonomous formation control 
such that agents within a formation can maintain an optimal position for specific phases of 
flight. 
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